#include "NormalAstar.h"


#define RIGHT { 0 , 1 }
#define LEFT  { 0 ,-1 }
#define UP    {-1 , 0 }
#define DOWN  { 1 , 0 }



CNormalAstar::CNormalAstar()
{
	for ( int i = 0 ; i < 800 ; i++ )
		for ( int j = 0 ; j < 600 ; j++ )
				terrain[i][j] = 0 ;


}

///////////////////////////////////////////////
// IsInOpen
// 
// Check if a Node is in the open list
// [return] : true if the Node is in the open list
//////////////////////////////////////////////
bool CNormalAstar::IsInOpen(POINT p)
{

	list<CNode>::iterator iternode ;

	for ( iternode = opennode.begin() ; iternode != opennode.end() ; iternode++ )
	{
		if ( iternode->p.x == p.x && iternode->p.y == p.y )
			return true ;
	}


return false ;
}
///////////////////////////////////////////////
// IsInClose
// 
// Check if a Node is in the close list
// [return] : true if the Node is in the close list
//////////////////////////////////////////////
bool CNormalAstar::IsInClose(POINT p)
{

	list<CNode>::iterator iternode ;

	for ( iternode = closenode.begin() ; iternode != closenode.end() ; iternode++ )
	{
		if ( iternode->p.x == p.x && iternode->p.y == p.y )
			return true ;
	}


return false ;
}
///////////////////////////////
// GetBetterNode
// 
// Get the node with the lowest
// totalCost
// [return] : the better CNode
///////////////////////////////
CNode CNormalAstar::GetBetterNode()
{
	double distmin = 999999 ;
	CNode best ;

	list<CNode>::iterator iternode;

	for ( iternode = opennode.begin() ; iternode != opennode.end() ; iternode++ )
	{
		if ( iternode->TotalCost < distmin )
		{
			if ( IsInClose(iternode->p) == false )
			{
				distmin = iternode->TotalCost ;
				
				best = *iternode ;
			}
		}
	}

return best ;
}



////////////////////////////////////////////////
// Astar
//
// Search the quickest way between 2 points
//
// [return] : The Node which countains the way
//            in its parent
////////////////////////////////////////////////
CNode *CNormalAstar::Astar(POINT dep , POINT arr )
{

	// Clear the 2 lists of Nodes
      opennode.clear() ;
	 closenode.clear() ;

	// Make the first Node
	// according to the departure point
	CNode *n = new CNode ;
	n->p = dep ;
	n->CostFromStart = 0 ;
	n->CostToEnd = CUtils::dist( n->p , arr );
	n->TotalCost = n->CostToEnd ;
	n->parent = NULL ;
	
	// Add the first node in the open list
	opennode.push_front(*n);

	while ( opennode.empty() == false )
	{
		// Get the better Point
		CNode *actual = new CNode ;
		*actual = GetBetterNode() ;

		// If this point is the arrival
		if ( CUtils::dist(actual->p , arr ) == 0 )
		{
			return actual ;
		}else
		{

			// list of accessibles points from the actual
			int direction[4][2] = {RIGHT,LEFT,DOWN,UP};

			// add the neighbour in the open list
			for ( int d = 0 ; d < 4 ; d++ )
			{
				int x = actual->p.x + direction[d][0] ;
				int y = actual->p.y + direction[d][1] ;

				// if this case is reachable
				if ( x >= 0 && x < WINDOW_WIDTH && y >= 0 && y < WINDOW_HEIGHT && terrain[x][y] != 2 )
				{
					CNode *neighbour = new CNode ;
					neighbour->p.x = x ;
					neighbour->p.y = y ;

					if ( IsInClose( neighbour->p ) == false  ) 
					{
						neighbour->CostFromStart = actual->CostFromStart  ;
						neighbour->CostToEnd = CUtils::dist ( neighbour->p , arr ) ;
						neighbour->TotalCost = neighbour->CostFromStart  + neighbour->CostToEnd ;
						
						neighbour->parent = actual ;
						// add the case in the open list
						opennode.push_front(*neighbour);
					}

					delete neighbour ;
				}

			}

		}
		// Add the node in the closed list
		closenode.push_front(*actual);

	 // Si qq'un me trouve pourquoi ce delete fait planter...
	 //	delete  actual ;
	}



return n ;
}

////////////////////////////////////////
// DoPath
//
// Do the path according to the node
// returned by Astar
////////////////////////////////////////
void CNormalAstar::DoPath(POINT dep,POINT arr)
{

	CNode *n = Astar(dep,arr);

	while ( n )
	{
		terrain[n->p.x][n->p.y] = 1 ;

	n = n->parent ;
	}


}


////////////////////////////
// Draw
//
// Draw the path
///////////////////////////
void CNormalAstar::Draw( HDC hdc )
{
	
		for ( int i = 0 ; i < 800 ; i++ )
			for ( int j = 0 ; j < 600 ; j++ )
				if (terrain[i][j] == 1)
					SetPixel(hdc,i,j,0x0000FF00);
				
	
}
//////////////////////////////////
// PrepareAstar
//
// Make the Poly cases uncaccessible
// BUGGED !!!!!!!!!!!!!
////////////////////////////////////
void CNormalAstar::PrepareAstar(list<CPolygone> listpoly)
{
	list<CPolygone>::iterator iterpoly;
	list<CLine>::iterator iterline;

	// Check in every poly
	for ( iterpoly = listpoly.begin() ; iterpoly != listpoly.end() ; iterpoly++)
	{
		for ( iterline = iterpoly->listline.begin() ; iterline != iterpoly->listline.end() ; iterline++)
		{
			for ( int x = iterline->Getbegin().x ; x <= iterline->Getend().x ; x++ )
			{
				terrain[x][(int)(iterline->Geta()*x+iterline->Getb())] = 2 ;
			}

		}

	}


}